import os

from ament_index_python import get_package_share_directory
from launch import LaunchDescription
from launch.actions import (DeclareLaunchArgument, IncludeLaunchDescription,
                            SetEnvironmentVariable)
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node


def generate_launch_description():
    use_sim_time = LaunchConfiguration('use_sim_time', default='false')
    bichon_slam_dir = get_package_share_directory('slam_toolbox')
    bichon_slam = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(bichon_slam_dir, 'launch', 'online_sync.launch.py')
        ),
    )
    parameters = [{
        'queue_size': 20,
        'frame_id': 'camera_link',
        'use_sim_time': use_sim_time,
        'subscribe_depth': True}]

    remappings = [
        ('odom', '/odom_combined'),
        ('rgb/image', '/camera/color/image_raw'),
        ('rgb/camera_info', '/camera/color/camera_info'),
        ('depth/image', '/camera/depth/image')]

    return LaunchDescription([
        bichon_slam,
        # Set env var to print messages to stdout immediately
        SetEnvironmentVariable('RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1'),

        # Launch arguments
        DeclareLaunchArgument(
            'use_sim_time', default_value='false',
            description='Use simulation (Gazebo) clock if true'),

        # Nodes to launch
        Node(
            package='rtabmap_ros', executable='rtabmap', output='screen',
            parameters=parameters,
            remappings=remappings,
            arguments=['-d']),
    ])
